#include "PID.h"  

/**************************************** PID计算  **********************/ 
//  Function Name  : PIDarithmetic  
//  Description    : PID  arithmetic of the controller   
//  Input: None *
//  Output: None * 
//  Return:   


/*******************外部传感器PID算法**********************************/  
 signed long int  PIDarithmetic_tuning(PID_TUN *pp)
 {      
   signed long int dError,Error;  
   Error = pp->SetPoint - pp->RealPoint;                  // 偏差
   pp->SumError += Error;
   dError = pp->LastError - pp->PrevError;          // 当前微分 
   pp->PrevError = pp->LastError;  
   pp->LastError = Error;  
  if(pp->SumError>10000)   //5000
  pp->SumError=10000;      //5000;
  else if(pp->SumError<0)  
  pp->SumError=0;   	 
   return ( (signed long int)(pp->Proportion * (float)Error)                 // 比例项 
          +(signed long int)((pp->Integral) * ((float)(pp->SumError)))           // 积分项  
	      +pp->Derivative * dError                // 微分项 
		  );
}  



/*******************外部传感器PID算法**********************************/  
 signed long int  PIDarithmetic_Out(PID *pp)
 {      
   signed long int dError,Error;  
	 float ErrorCoeeff;//误差衰减系数
   Error = pp->SetPoint - pp->RealPoint;                  // 偏差
	 ErrorCoeeff=Error;
	 if(ErrorCoeeff<pp->Integral_cycle)
	 {
      ErrorCoeeff=ErrorCoeeff*(1-(ErrorCoeeff/pp->Integral_cycle));
		  pp->SumError += (signed long int)ErrorCoeeff;
   }
	 else
	 ErrorCoeeff=0;	 
//    dError = pp->LastError - pp->PrevError;          // 当前微分 
   pp->PrevError = pp->LastError;  
   pp->LastError = Error;  
   dError = pp->LastError - pp->PrevError;          // 当前微分 	 
	 if(dError>50)//微分异常不做处理
	 dError=0;
	 if(dError<-50)
	dError=0; 
	 if(pp->SetPoint>pp->RealPoint) 
	 {
			if(dError<0)
			{
       pp->SumError_max =pp->SumError; 
      }
			if(dError>0)
			{
        if(pp->FirstCycleFlag==2)
				pp->FirstCycleFlag=3;	
				
				if(pp->T_Adj)
				{
					 pp->T_Adj=0;
					if((pp->N_Error_max1<-12)&&(pp->N_Error_max2<-12))
					{
						if(pp->N_Error_max1<pp->N_Error_max2+4)
						{
              pp->Integral=pp->Integral*0.75;
							pp->Derivative=pp->Derivative*1.1; //20200410
            }
					}
          pp->N_Error_max2=pp->N_Error_max1;
					pp->N_Error_max1=0;
        }	
      }
   }		 
  if(pp->SumError>15000)   
  pp->SumError=15000;     
  else if(pp->SumError<0)  
  pp->SumError=0;   	 
	 if(pp->RealPoint>pp->SetPoint) 
	 {
			if(dError>0)
			{
				pp->SumError =pp->SumError-(signed long int)ErrorCoeeff;
      }
			else if(dError<-3)
			{ 
				pp->SumError =pp->SumError- (signed long int)ErrorCoeeff;
      }
			//20200513
			else if (dError==0)
			{
       	pp->SumError =pp->SumError- ((signed long int)ErrorCoeeff/2);
      }
			
			if(dError<1)
			{
          if(pp->RealPoint>pp->RealPoint_max)
				  pp->RealPoint_max=pp->RealPoint;	
				  if(pp->FirstCycleFlag==1)
					pp->FirstCycleFlag=2;	
					
				   if(pp->N_Error_max1>Error)
						pp->N_Error_max1=Error;
					  pp->T_Adj=1;
			}
				
   }	
   return ( (signed long int)(pp->Proportion * (float)Error)                 // 比例项 
          +(signed long int)((pp->Integral) * ((float)(pp->SumError)))           // 积分项  
	      +pp->Derivative * dError                // 微分项 
		  );
}  
/*******************内部传感器PID算法****************************************/ 
signed long int  PIDarithmetic_In(PID *pp)
 {      
   signed long int dError,Error;  
   Error = pp->SetPoint - pp->RealPoint;                  // 偏差  
	 
   pp->SumError += Error;                            // 积分  
	 
   dError = pp->LastError - pp->PrevError;          // 当前微分 
	 
	 if(dError>100)//微分异常不做处理
	 dError=0;
	 if(dError<-100)
	  dError=0; 
   pp->PrevError = pp->LastError;  
   pp->LastError = Error;  
	 
  if(pp->SumError>20000)  //(B) 5000
  pp->SumError=20000;     
  else if(pp->SumError<0) //(C) 
  pp->SumError=0;  
	
  return ( (signed long int)(pp->Proportion * (float)Error)                 // 比例项 
          +(signed long int)((pp->Integral) * ((float)(pp->SumError)))           // 积分项  
	      +pp->Derivative * dError                // 微分项 
		  );   
} 
/*******************MOTER_PID算法****************************************/ 

signed long int  PID_moter(PID2 *pp)
 {      
   signed long int ddError,dError,Error,temp[3];  
   Error = pp->SetPoint - pp->RealPoint;         // 偏差                                                   
   pp->BePrevError=pp->PrevError; //Error2
   pp->PrevError = pp->LastError;//Error1  
   pp->LastError = Error;  	//Error0
   dError = pp->LastError - pp->PrevError; //一阶微分 
   ddError=	(pp->BePrevError+pp->LastError)-2*(pp->PrevError);//二阶微分
   if(dError<-100)
		dError=0;
	  if(ddError<-100)
		ddError=0;
   temp[0]=pp->Proportion*Error;  //比例项
   temp[1]=pp->Integral*dError;   //积分项
   temp[2]=pp->Derivative*ddError; // 微分项
  return ( temp[0]+temp[1]+temp[2]);                  
} 


/*						  

Kc=8;
Tc=15*30s;
T=6;
kp=0.65kc;
Ti=0.5Tc;
Td=0.12Tc;
Ki=Kp*T/Ti
Kd=Kp*Td/T;
*/

/*
水
T=10S
Tc=50
Ti=8*Tc=400;
Td=0.8*Tc=40;
kc=8
kp=0.65kc=5;
ki=kp*10/400=kp*0.025=0.125;
kd=kp*40/10=kp*4=20;

P->ERR=ERR(1-ERR/100)

*/


/*

硅油
T=10S
Tc=35
Ti=8*Tc=280;
Td=0.8*Tc=28;
kc=5.6
kp=0.65kc=3.64;
ki=kp*10/280=kp*0.041=0.13;
kd=kp*28/10=kp*2.8=10;

P->ERR=ERR(1-ERR/100)
*/

